#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <malloc.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <i264e_common.h>
#include <i264e_pixel.h>
#include <assert.h>
#include <hwicodec.h>
#include <hwicodec_common.h>
//#define SIGN_I264E_VERSION
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif

void i264e_init_func(void *init_arg)
{
	//i264e_log(NULL, C_LOG_INFO, "init_arg is %lx\n", (intptr_t)init_arg);
}

i264e_t *i264e_init(i264e_param_t *param)
{
	int i = 0;
	i264e_t *h = malloc(sizeof(i264e_t));

	if (h == NULL) {
		i264e_log(NULL, C_LOG_ERROR, "malloc i264e handler h failed\n");
		goto err_malloc_i264e;
	}
	memset(h, 0, sizeof(i264e_t));
	h->thread.h = h;

	/* here I suppose the param is valid */
	memcpy(&h->common.param, param, sizeof(i264e_param_t));

	if (i264e_validate_parameters(h) < 0) {
		goto err_invalidate_parameters;
	}
    i264e_sps_init(h->common.sps, h->common.param.i_sps_id, &h->common.param);
    i264e_pps_init(h->common.pps, h->common.param.i_sps_id, &h->common.param, h->common.sps);

    i264e_validate_levels(h, 1);

    i264e_bitstream_init(&h->common.bsf );
    c_mc_init(&h->common.mc);

    if(h->common.param.b_cabac) {
        i264e_cabac_init(h);
	} else {
		i264e_log(h, C_LOG_ERROR, "[func:%s,line:%d]unsupport cabac\n",
				__func__, __LINE__);
		goto err_unsupport_cabac;
	}

	i264e_init_skip_header(&h->common.skip_h, h->common.param.i_gop,
			h->common.param.hskip.i_skip_type,
			h->common.param.hskip.m, h->common.param.hskip.n,
			h->common.param.hskip.max_same_scenecnt,
			h->common.param.i_max_skip_type, &h->common.rdframe_cnt,
			&h->common.rdsframe_cnt, h->common.param.b_mref, h->common.param.b_buf_share);
	h->common.encframe_cnt = h->common.param.b_user_fenc ? 0 : h->common.param.i_threads;

    /* Must be volatile or else GCC will optimize it out. */
    volatile int temp = 392;
    if( i264e_clz( temp ) != 23 )
    {
        i264e_log( h, C_LOG_ERROR, "CLZ test failed: i264e has been miscompiled!\n" );
        goto err_compiled;
    }

    h->thread.p.i_global_qp = h->common.param.rc.i_qp_constant;

    h->thread.p.out.i_nal = 0;
    h->thread.p.out.i_bitstream = C_MAX(1000000, C_ALIGN(h->common.param.i_width, 16)
			* C_ALIGN(h->common.param.i_height, 16) + 4096 + 2 * 128);

	if (i264e_threadpool_init(&h->common.threadpool, h->common.param.i_threads,
				i264e_init_func, NULL) < 0) {
		i264e_log(h, C_LOG_ERROR, "[func:%s,line:%d]threadpool init failed\n",
				__func__, __LINE__);
		goto err_threadpool_init;
	}

	h->thread.h = h;
	h->threads[0] = &h->thread;
	for (i = 1; i < h->common.param.i_threads; i++) {
		h->threads[i] = malloc(sizeof(i264e_thread_t));
		if (h->threads[i] == NULL) {
			i264e_log(h, C_LOG_ERROR, "[func:%s,line:%d]malloc thread[%d] failed\n",
					__func__, __LINE__, i);
			goto err_malloc_threads;
		}
	}

	for (i = 0; i < h->common.param.i_threads; i++) {
        int init_nal_count = h->common.param.i_slice_count + 3;
		if (i > 0) {
			memcpy(h->threads[i], &h->thread, sizeof(i264e_thread_t));
		}
		h->threads[i]->p.fenc = &h->common.encframe[i];
		h->threads[i]->p.fpad = &h->common.padframe[i];

		if (i264e_pthread_mutex_init(&h->threads[i]->p.mutex, NULL) < 0) {
			i264e_log(h, C_LOG_ERROR, "pthread_mutex_init[%d] failed\n", i);
			goto err_pthread_mutex_init;
		}

		if (i264e_pthread_cond_init(&h->threads[i]->p.cond, NULL) < 0) {
			i264e_log(h, C_LOG_ERROR, "pthread_cond_init[%d] failed\n", i);
			goto err_pthread_cond_init;
		}

		if (i264e_pthread_cond_init(&h->threads[i]->p.cond_slice, NULL) < 0) {
			i264e_log(h, C_LOG_ERROR, "pthread_cond_init slice[%d] failed\n", i);
			goto err_pthread_cond_init_slice;
		}

		if (i264e_icodec_io_init(h, &h->threads[i]->p.icodec_io) < 0) {
			i264e_log(h, C_LOG_ERROR, "icodec_io_init[%d] failed\n", i);
			goto err_icodec_io_init;
		}

		if (h->common.param.b_user_nalbuffer == 0) {
			if ((h->threads[i]->p.out.p_bitstream = memalign(C_NATIVE_ALIGN,
							h->threads[i]->p.out.i_bitstream)) == NULL) {
				i264e_log(h, C_LOG_ERROR, "malloc thread[%d] p_bitstream failed\n", i);
				goto err_memalign_p_bitstream;
			}
		}

        if ((h->threads[i]->p.out.nal = memalign(C_NATIVE_ALIGN,
						init_nal_count * sizeof(i264e_nal_t))) == NULL) {
			i264e_log(h, C_LOG_ERROR, "malloc thread[%d] nal failed\n", i);
			goto err_memalign_nal;
		}

        h->threads[i]->p.out.i_nals_allocated = init_nal_count;
	}

	if (h->common.param.b_user_nalbuffer == 0) {
		for (i = 0; i < h->common.param.i_threads; i++) {
			/* +4 for startcode, +64 for nal_escape assembly padding */
			h->threads[i]->p.nal_buffer_size = h->threads[i]->p.out.i_bitstream * 3/2 + 4 + 64;
			if ((h->threads[i]->p.nal_buffer = memalign(NATIVE_ALIGN, h->threads[i]->p.nal_buffer_size)) == NULL) {
				i264e_log( h, C_LOG_ERROR, "memalign nal buffer failed\n");
				goto err_memalign_nal_buffer;
			}
		}
	}

	if (h->common.param.i_threads > 1) {
		h->cur = h->threads[h->common.param.i_threads - 1];
		h->cur->p.threads_info.cur = h->cur;
		h->cur->p.threads_info.oldest = h->threads[0];
	} else {
		h->cur = &h->thread;
		h->cur->p.threads_info.cur = h->cur->p.threads_info.oldest = h->cur;
	}
	h->cur->ip.i_frame = -1;
	memcpy(h->cur->ip.roi, h->common.param.roi, 8*sizeof(c_roi_t));
	h->cur->ip.i_thread_phase = h->common.param.i_threads - 1;
	h->cur->ip.i_idr_pic_id = 0;
	h->cur->p.b_thread_active = 0;
	h->cur->p.b_thread_done = 0;
    h->cur->p.threads_info.oldest->op.i_prev_duration =
		((uint64_t)h->common.param.i_fps_den * h->common.sps->vui.i_time_scale)
		/ ((uint64_t)h->common.param.i_fps_num * h->common.sps->vui.i_num_units_in_tick);

	/* reconfig init should before i264e_icodec_init, for empty init */
	if (i264e_reconfig_init(h)) {
		i264e_log(h, C_LOG_ERROR, "i264e_reconfig_init failed\n");
		goto err_i264e_rcfg_init;
	}

	if (i264e_icodec_init(&h->thread) < 0) {
		i264e_log(h, C_LOG_ERROR, "icodec_init failed\n");
		goto err_icodec_init;
	}

	if (i264e_icodec_request_vbinfo(&h->thread)) {
		i264e_log(h, C_LOG_ERROR, "icodec_get_vbinfo failed\n");
		goto err_icodec_request_vbinfo;
	}
	h->thread.p.icodec_io.protocol.h264e.H264E_SliceInfoSize = h->thread.h->common.H264E_SliceInfoSize;
	h->thread.p.icodec_io.protocol.h264e.H264E_SliceInfo = calloc(1, h->thread.h->common.H264E_SliceInfoSize);
	assert(h->thread.p.icodec_io.protocol.h264e.H264E_SliceInfo);
    const char *profile = h->common.sps->i_profile_idc == PROFILE_BASELINE ? "Constrained Baseline" :
                          h->common.sps->i_profile_idc == PROFILE_MAIN ? "Main" :
                          h->common.sps->i_profile_idc == PROFILE_HIGH ? "High" :
                          h->common.sps->i_profile_idc == PROFILE_HIGH10 ?
						  (h->common.sps->b_constraint_set3 == 1 ? "High 10 Intra" : "High 10") :
                          h->common.sps->i_profile_idc == PROFILE_HIGH422 ?
						  (h->common.sps->b_constraint_set3 == 1 ? "High 4:2:2 Intra" :
						   "High 4:2:2") :
                          h->common.sps->b_constraint_set3 == 1 ? "High 4:4:4 Intra" :
						  "High 4:4:4 Predictive";
    char level[4];
    snprintf(level, sizeof(level), "%d.%d", h->common.sps->i_level_idc/10,
			h->common.sps->i_level_idc%10);
    if(h->common.sps->i_level_idc == 9 || (h->common.sps->i_level_idc == 11
			&& h->common.sps->b_constraint_set3 &&
			(h->common.sps->i_profile_idc == PROFILE_BASELINE
			 || h->common.sps->i_profile_idc == PROFILE_MAIN)))
        strcpy(level, "1b");

    if(h->common.sps->i_profile_idc < PROFILE_HIGH10) {
        i264e_log(h, C_LOG_INFO, "profile %s, level %s\n", profile, level );
    } else {
        static const char * const subsampling[4] = {"4:0:0", "4:2:0", "4:2:2", "4:4:4"};
        i264e_log(h, C_LOG_INFO, "profile %s, level %s, %s %d-bit\n",
            profile, level, subsampling[h->common.sps->i_chroma_format_idc], C_BIT_DEPTH);
    }

    memset(&h->common.eprc_t21, 0, sizeof(VPU_RC_S_T21));
	i264e_ratecontrol_init(h, -1);
	return h;

err_icodec_request_vbinfo:
	i264e_icodec_deinit(&h->thread);
err_icodec_init:
err_i264e_rcfg_init:
	i = h->common.param.i_threads;
err_memalign_nal_buffer:
	for (--i; i >= 0; i--) {
		free(h->threads[i]->p.nal_buffer);
	}
	i = h->common.param.i_threads;
err_memalign_nal:
	if (i < h->common.param.i_threads) {
		if (h->common.param.b_user_nalbuffer == 0) {
			free(h->threads[i]->p.out.p_bitstream);
		}
	}
err_memalign_p_bitstream:
	i264e_icodec_io_deinit(h, &h->threads[i]->p.icodec_io);
err_icodec_io_init:
	i264e_pthread_cond_destroy(&h->threads[i]->p.cond_slice);
err_pthread_cond_init_slice:
	i264e_pthread_cond_destroy(&h->threads[i]->p.cond);
err_pthread_cond_init:
	i264e_pthread_mutex_destroy(&h->threads[i]->p.mutex);
err_pthread_mutex_init:
	for (--i; i >= 0; i--) {
		i264e_icodec_io_deinit(h, &h->threads[i]->p.icodec_io);
		i264e_pthread_cond_destroy(&h->threads[i]->p.cond_slice);
		i264e_pthread_cond_destroy(&h->threads[i]->p.cond);
		i264e_pthread_mutex_destroy(&h->threads[i]->p.mutex);
		free(h->threads[i]->p.out.nal);
		if (h->common.param.b_user_nalbuffer == 0) {
			free(h->threads[i]->p.out.p_bitstream);
		}
	}
	i = h->common.param.i_threads;
err_malloc_threads:
	for (--i; i >= 1; i--) {
		free(h->threads[i]);
		h->threads[i] = NULL;
	}
	i264e_threadpool_delete(h->common.threadpool);
err_threadpool_init:
err_compiled:
err_unsupport_cabac:
err_invalidate_parameters:
	free(h);
	h = NULL;
err_malloc_i264e:
	return NULL;
}

void i264e_deinit(i264e_t *h)
{
	int i = 0;
	int64_t i_yuv_size = h->common.param.i_width*h->common.param.i_height*3/2;
	if (h == NULL) {
		return;
	}

	i264e_ratecontrol_deinit(h);

    /* Slices used and PSNR */
	i264e_thread_t * cur = h->cur;
    for(i = 0; i < 3; i++ )
    {
        static const uint8_t slice_order[] = { I264E_SLICE_TYPE_I, I264E_SLICE_TYPE_P, I264E_SLICE_TYPE_B };
        int i_slice = slice_order[i];
		if(cur->op.stat.i_frame_count[i_slice] > 0)
        {
            int i_count = cur->op.stat.i_frame_count[i_slice];
            double dur =  cur->op.stat.f_frame_duration[i_slice];
            if(h->common.param.analyse.b_psnr)
            {
                i264e_log( h, C_LOG_INFO,
                          "frame %c:%-5d PSNR Mean Y:%5.2f U:%5.2f V:%5.2f Avg:%5.2f Global:%5.2f\n",
						  slice_type_to_char[i_slice],
						  i_count,
                          cur->op.stat.f_psnr_mean_y[i_slice] / dur,
						  cur->op.stat.f_psnr_mean_u[i_slice] / dur,
						  cur->op.stat.f_psnr_mean_v[i_slice] / dur,
                          cur->op.stat.f_psnr_average[i_slice] / dur,
                          i264e_psnr(cur->op.stat.f_ssd_global[i_slice], dur * i_yuv_size ) );
            }
        }
    }
	int frame_total_cnt = cur->op.stat.i_frame_count[I264E_SLICE_TYPE_I] +
							cur->op.stat.i_frame_count[I264E_SLICE_TYPE_P] +
							cur->op.stat.i_frame_count[I264E_SLICE_TYPE_B];
    if(frame_total_cnt > 0) {
#define SUM3(p) (p[I264E_SLICE_TYPE_I] + p[I264E_SLICE_TYPE_P] + p[I264E_SLICE_TYPE_B])
        const double duration = cur->op.stat.f_frame_duration[I264E_SLICE_TYPE_I] +
                                cur->op.stat.f_frame_duration[I264E_SLICE_TYPE_P] +
                                cur->op.stat.f_frame_duration[I264E_SLICE_TYPE_B];
        float f_bitrate = SUM3(cur->op.stat.i_frame_size) / duration / 125;

        if(h->common.param.analyse.b_psnr)
        {
            i264e_log(h, C_LOG_INFO,
                      "PSNR Mean Y:%6.3f U:%6.3f V:%6.3f Avg:%6.3f Global:%6.3f kb/s:%.2f\n",
                      SUM3(cur->op.stat.f_psnr_mean_y) / duration,
                      SUM3(cur->op.stat.f_psnr_mean_u) / duration,
                      SUM3(cur->op.stat.f_psnr_mean_v) / duration,
                      SUM3(cur->op.stat.f_psnr_average) / duration,
                      i264e_psnr(SUM3(cur->op.stat.f_ssd_global), duration * i_yuv_size),
                      f_bitrate);
        }
        else
            i264e_log(h, C_LOG_INFO, "kb/s:%.2f\n", f_bitrate);
	}

	i264e_icodec_release_vbinfo(&h->thread);
	i264e_icodec_deinit(&h->thread);

	for (i = h->common.param.i_threads - 1; i >= 0; i--) {
		i264e_icodec_io_deinit(h, &h->threads[i]->p.icodec_io);
		i264e_pthread_cond_destroy(&h->threads[i]->p.cond_slice);
		i264e_pthread_cond_destroy(&h->threads[i]->p.cond);
		i264e_pthread_mutex_destroy(&h->threads[i]->p.mutex);

		if (h->threads[i] && h->threads[i]->p.out.nal) {
			free(h->threads[i]->p.out.nal);
		}

		if (h->threads[i] && h->threads[i]->p.out.p_bitstream) {
			if (h->common.param.b_user_nalbuffer == 0) {
				free(h->threads[i]->p.out.p_bitstream);
			}
		}

        if (h->threads[i] && h->threads[i]->p.tmp_bs_space) {
            free(h->threads[i]->p.tmp_bs_space);
        }
	}

	for (i = h->common.param.i_threads - 1; i >= 0; i--) {
		if (h->threads[i] && h->threads[i]->p.nal_buffer) {
			free(h->threads[i]->p.nal_buffer);
		}
	}
	for (i = h->common.param.i_threads - 1; i >= 1; i--) {
		if (h->threads[i]) {
			free(h->threads[i]);
			h->threads[i] = NULL;
		}
	}

	if (h->thread.p.icodec_io.protocol.h264e.H264E_SliceInfo) {
		free(h->thread.p.icodec_io.protocol.h264e.H264E_SliceInfo);
	}

	i264e_threadpool_delete(h->common.threadpool);

	i264e_reconfig_deinit(h);
	free(h);
}

static void i264e_thread_notify_send(i264e_thread_t *t)
{
	i264e_pthread_mutex_lock(&t->p.mutex);
	t->p.b_thread_active = 0;
	i264e_pthread_cond_broadcast(&t->p.cond);
	i264e_pthread_mutex_unlock(&t->p.mutex);
}

static void i264e_thread_notify_wait(i264e_thread_t *t, volatile int *flag)
{
	i264e_pthread_mutex_lock(&t->p.mutex);
	if (t->p.b_thread_active) {
		i264e_pthread_cond_wait(&t->p.cond, &t->p.mutex);
	}
	*flag = 0;
	i264e_pthread_mutex_unlock(&t->p.mutex);
}

static void i264e_thread_slice_notify_send(i264e_thread_t *t)
{
	i264e_pthread_mutex_lock(&t->p.mutex);
	t->p.b_thread_done = 1;
	i264e_pthread_cond_broadcast(&t->p.cond_slice);
	i264e_pthread_mutex_unlock(&t->p.mutex);
}

static void i264e_thread_slice_notify_wait(i264e_thread_t *t)
{
	i264e_pthread_mutex_lock(&t->p.mutex);
	if (t->p.b_thread_done == 0) {
		i264e_pthread_cond_wait(&t->p.cond_slice, &t->p.mutex);
	}
	t->p.b_thread_done = 0;
	i264e_pthread_mutex_unlock(&t->p.mutex);
}

void i264e_threadpool_wait_all(i264e_t *h)
{
	int i = 0;
	i264e_thread_t *oldest = h->cur->p.threads_info.oldest;

    for(i = 0; i < h->common.param.i_threads - 1; i++) {
        if(oldest->p.b_thread_used) {
			i264e_thread_notify_wait(oldest, &oldest->p.threads_info.prev->p.b_oldest_thread_used);
        }
		oldest = oldest->p.threads_info.oldest;
	}
}

static void i264e_slice_header_init(i264e_thread_t *t, i264e_slice_header_t *sh,
                                    i264e_sps_t *sps, i264e_pps_t *pps,
                                    int i_idr_pic_id, int i_frame, int i_qp)
{
    i264e_param_t *param = &t->h->common.param;

    /* First we fill all fields */
    sh->sps = sps;
    sh->pps = pps;

    sh->i_first_mb  = 0;
    sh->i_last_mb   = t->h->common.sps->i_mb_width * t->h->common.sps->i_mb_height - 1;
    sh->i_pps_id    = pps->i_id;

    sh->i_frame = i_frame;

    sh->b_mbaff = 0;
    sh->b_field_pic = 0;    /* no field support for now */
    sh->b_bottom_field = 0; /* not yet used */

    sh->i_idr_pic_id = i_idr_pic_id;

    /* poc stuff, fixed later */
    sh->i_poc = 0;
    sh->i_delta_poc_bottom = 0;
    sh->i_delta_poc[0] = 0;
    sh->i_delta_poc[1] = 0;

    sh->i_redundant_pic_cnt = 0;


    sh->i_cabac_init_idc = param->i_cabac_init_idc;

    sh->i_qp = SPEC_QP(i_qp);
    sh->i_qp_delta = sh->i_qp - pps->i_pic_init_qp;
    sh->b_sp_for_swidth = 0;
    sh->i_qs_delta = 0;

    int deblock_thresh = i_qp + 2 * C_MIN(param->i_deblocking_filter_alphac0, param->i_deblocking_filter_beta);
    /* If effective qp <= 15, deblocking would have no effect anyway */
    if( param->b_deblocking_filter && (15 < deblock_thresh))
        sh->i_disable_deblocking_filter_idc = 0;
    else
        sh->i_disable_deblocking_filter_idc = 1;
    sh->i_alpha_c0_offset = param->i_deblocking_filter_alphac0 << 1;
    sh->i_beta_offset = param->i_deblocking_filter_beta << 1;
}

static void i264e_slice_header_write(bs_t *s, i264e_slice_header_t *sh, int i_nal_ref_idc )
{
	int i = 0, j = 0;

    if( sh->b_mbaff )
    {
        int first_x = sh->i_first_mb % sh->sps->i_mb_width;
        int first_y = sh->i_first_mb / sh->sps->i_mb_width;
        assert( (first_y&1) == 0 );
        bs_write_ue( s, (2*first_x + sh->sps->i_mb_width*(first_y&~1) + (first_y&1)) >> 1 );
    }
    else
        bs_write_ue( s, sh->i_first_mb );

    bs_write_ue( s, sh->i_type);   /* same type things */
    bs_write_ue( s, sh->i_pps_id );
    bs_write( s, sh->sps->i_log2_max_frame_num, sh->i_frame & ((1<<sh->sps->i_log2_max_frame_num)-1) );

    if( !sh->sps->b_frame_mbs_only )
    {
        bs_write1( s, sh->b_field_pic );
        if( sh->b_field_pic )
            bs_write1( s, sh->b_bottom_field );
    }

    if( sh->i_idr_pic_id >= 0 ) /* NAL IDR */
        bs_write_ue( s, sh->i_idr_pic_id );

    if( sh->sps->i_poc_type == 0 )
    {
        bs_write( s, sh->sps->i_log2_max_poc_lsb, sh->i_poc & ((1<<sh->sps->i_log2_max_poc_lsb)-1) );
        if( sh->pps->b_pic_order && !sh->b_field_pic )
            bs_write_se( s, sh->i_delta_poc_bottom );
    }

    if( sh->pps->b_redundant_pic_cnt )
        bs_write_ue( s, sh->i_redundant_pic_cnt );

    if( sh->i_type == I264E_SLICE_TYPE_B )
        bs_write1( s, sh->b_direct_spatial_mv_pred );

    if( sh->i_type == I264E_SLICE_TYPE_P || sh->i_type == I264E_SLICE_TYPE_B )
    {
        bs_write1( s, sh->b_num_ref_idx_override );
        if( sh->b_num_ref_idx_override )
        {
            bs_write_ue( s, sh->i_num_ref_idx_l0_active - 1 );
            if( sh->i_type == I264E_SLICE_TYPE_B )
                bs_write_ue( s, sh->i_num_ref_idx_l1_active - 1 );
        }
    }

    /* ref pic list reordering */
    if( sh->i_type != I264E_SLICE_TYPE_I )
    {
        bs_write1( s, sh->b_ref_pic_list_reordering[0] );
        if( sh->b_ref_pic_list_reordering[0] )
        {
            for( i = 0; i < sh->i_num_ref_idx_l0_active; i++ )
            {
                bs_write_ue( s, sh->ref_pic_list_order[0][i].idc );
                bs_write_ue( s, sh->ref_pic_list_order[0][i].arg );
            }
            bs_write_ue( s, 3 );
        }
    }
    if( sh->i_type == I264E_SLICE_TYPE_B )
    {
        bs_write1( s, sh->b_ref_pic_list_reordering[1] );
        if( sh->b_ref_pic_list_reordering[1] )
        {
            for( i = 0; i < sh->i_num_ref_idx_l1_active; i++ )
            {
                bs_write_ue( s, sh->ref_pic_list_order[1][i].idc );
                bs_write_ue( s, sh->ref_pic_list_order[1][i].arg );
            }
            bs_write_ue( s, 3 );
        }
    }

    sh->b_weighted_pred = 0;
    if( sh->pps->b_weighted_pred && sh->i_type == I264E_SLICE_TYPE_P )
    {
        sh->b_weighted_pred = sh->weight[0][0].weightfn || sh->weight[0][1].weightfn || sh->weight[0][2].weightfn;
        /* pred_weight_table() */
        bs_write_ue( s, sh->weight[0][0].i_denom );
        bs_write_ue( s, sh->weight[0][1].i_denom );
        for( i = 0; i < sh->i_num_ref_idx_l0_active; i++ )
        {
            int luma_weight_l0_flag = !!sh->weight[i][0].weightfn;
            int chroma_weight_l0_flag = !!sh->weight[i][1].weightfn || !!sh->weight[i][2].weightfn;
            bs_write1( s, luma_weight_l0_flag );
            if( luma_weight_l0_flag )
            {
                bs_write_se( s, sh->weight[i][0].i_scale );
                bs_write_se( s, sh->weight[i][0].i_offset );
            }
            bs_write1( s, chroma_weight_l0_flag );
            if( chroma_weight_l0_flag )
            {
                for( j = 1; j < 3; j++ )
                {
                    bs_write_se( s, sh->weight[i][j].i_scale );
                    bs_write_se( s, sh->weight[i][j].i_offset );
                }
            }
        }
    }
    else if( sh->pps->b_weighted_bipred == 1 && sh->i_type == I264E_SLICE_TYPE_B )
    {
      /* TODO */
    }

    if( i_nal_ref_idc != 0 )
    {
        if( sh->i_idr_pic_id >= 0 )
        {
            bs_write1( s, 0 );  /* no output of prior pics flag */
            bs_write1( s, sh->i_mmco_command_count > 0);  /* long term reference flag */
        }
        else
        {
            bs_write1( s, sh->i_mmco_command_count > 0 ); /* adaptive_ref_pic_marking_mode_flag */
            if( sh->i_mmco_command_count > 0 )
            {
                for( i = 0; i < sh->i_mmco_command_count; i++ )
                {
                    bs_write_ue( s, sh->mmco[i].idc ); /* mark short term ref as unused */
                    bs_write_ue( s, sh->mmco[i].arg );
                }
                bs_write_ue( s, 0 ); /* end command list */
            }
        }
    }

    if( sh->pps->b_cabac && sh->i_type != I264E_SLICE_TYPE_I )
        bs_write_ue( s, sh->i_cabac_init_idc );

    bs_write_se( s, sh->i_qp_delta );      /* slice qp delta */

    if( sh->pps->b_deblocking_filter_control )
    {
        bs_write_ue( s, sh->i_disable_deblocking_filter_idc );
        if( sh->i_disable_deblocking_filter_idc != 1 )
        {
            bs_write_se( s, sh->i_alpha_c0_offset >> 1 );
            bs_write_se( s, sh->i_beta_offset >> 1 );
        }
    }
}

#if 0
/* If we are within a reasonable distance of the end of the memory allocated for the bitstream, */
/* reallocate, adding an arbitrary amount of space. */
static int i264e_bitstream_check_buffer_internal(i264e_thread_t *t, int size, int b_cabac, int i_nal)
{
	int i = 0;
    if((b_cabac && (t->p.cabac.p_end - t->p.cabac.p < size)) ||
        (t->p.out.bs.p_end - t->p.out.bs.p < size))
    {
        int buf_size = t->p.out.i_bitstream + size;
        uint8_t *buf = NULL;
        intptr_t delta = buf - t->p.out.p_bitstream;

        if ((buf = memalign(buf_size, C_NATIVE_ALIGN)) == NULL) {
			i264e_log(t->h, C_LOG_ERROR, "memalign p_bitstream failed\n");
			goto err_memalign_p_bitstream;
		}
		memcpy(buf, t->p.out.p_bitstream, t->p.out.i_bitstream);

        t->p.out.bs.p_start += delta;
        t->p.out.bs.p += delta;
        t->p.out.bs.p_end = buf + buf_size;

        t->p.cabac.p_start += delta;
        t->p.cabac.p += delta;
        t->p.cabac.p_end = buf + buf_size;

        for(i = 0; i <= i_nal; i++) {
            t->p.out.nal[i].p_payload += delta;
		}

        free(t->p.out.p_bitstream);
        t->p.out.p_bitstream = buf;
        t->p.out.i_bitstream = buf_size;
    }
    return 0;

err_memalign_p_bitstream:
	return -1;
}

static int i264e_bitstream_check_buffer(i264e_thread_t *t)
{
    int max_row_size = 2500 * t->h->common.sps->i_mb_width;
    return i264e_bitstream_check_buffer_internal(t, max_row_size, t->h->common.param.b_cabac,
			t->p.out.i_nal);
}
#endif

int i264e_slice_init(i264e_thread_t *t)
{
    /* ------------------------ Create slice header  ----------------------- */
    if(t->p.i_type == I264E_TYPE_IDR) {
        i264e_slice_header_init(t, &t->p.sh, t->h->common.sps, t->h->common.pps,
				t->ip.i_idr_pic_id, t->ip.i_frame, t->p.i_global_qp);
		t->ip.i_idr_pic_id ^= 1;
    } else {
        i264e_slice_header_init(t, &t->p.sh, t->h->common.sps, t->h->common.pps,
				-1, t->ip.i_frame, t->p.i_global_qp);
    }

    t->p.fdec->i_frame = t->p.sh.i_frame;

    if(t->h->common.sps->i_poc_type == 0)
    {
        t->p.sh.i_poc = t->p.i_poc;
		t->p.sh.i_delta_poc_bottom = 0;
        t->p.fdec->i_delta_poc[0] = t->p.sh.i_delta_poc_bottom == -1;
        t->p.fdec->i_delta_poc[1] = t->p.sh.i_delta_poc_bottom ==  1;
    }

	return 0;
}

/* internal usage */
void i264e_nal_start(i264e_thread_t *t, int i_type, int i_ref_idc)
{
    i264e_nal_t *nal = &t->p.out.nal[t->p.out.i_nal];

    nal->i_ref_idc        = i_ref_idc;
    nal->i_type           = i_type;
    nal->b_long_startcode = 1;

    nal->i_payload= 0;
    nal->p_payload= &t->p.out.p_bitstream[bs_pos(&t->p.out.bs) / 8];
    nal->i_padding= 0;
}

/* if number of allocated nals is not enough, re-allocate a larger one. */
static int i264e_nal_check_buffer(i264e_thread_t *t)
{
    if(t->p.out.i_nal >= t->p.out.i_nals_allocated)
    {
        i264e_nal_t *new_out = malloc(sizeof(i264e_nal_t)*(t->p.out.i_nals_allocated*2));
        if(!new_out ) {
			i264e_log(t->h, C_LOG_ERROR, "malloc i264e_nal_t failed\n");
			goto err_malloc_nal;
		}
        memcpy(new_out, t->p.out.nal, sizeof(i264e_nal_t)*(t->p.out.i_nals_allocated));
        free(t->p.out.nal);
        t->p.out.nal = new_out;
        t->p.out.i_nals_allocated *= 2;
    }

    return 0;

err_malloc_nal:
	return -1;
}

int i264e_nal_end(i264e_thread_t *t)
{
    i264e_nal_t *nal = &t->p.out.nal[t->p.out.i_nal];
    uint8_t *end = &t->p.out.p_bitstream[bs_pos(&t->p.out.bs )/8];
    nal->i_payload = end - nal->p_payload;
    /* Assembly implementation of nal_escape reads past the end of the input.
     * While undefined padding wouldn't actually affect the output, it makes valgrind unhappy. */
    memset(end, 0xff, 64);
    t->p.out.i_nal++;

    return i264e_nal_check_buffer(t);
}

static int i264e_check_encapsulated_buffer(i264e_thread_t *t, int start, int previous_nal_size,
		int necessary_size)
{
	int i = 0;
	/* user make sure the buffer is large enough */
	if (1 == t->h->common.param.b_user_nalbuffer)
		return 0;
    if( t->p.nal_buffer_size < necessary_size) {
        necessary_size *= 2;
        uint8_t *buf = malloc(necessary_size);
        if(!buf) {
			i264e_log(t->h, C_LOG_ERROR, "malloc %d size nal_buffer failed\n", necessary_size);
			goto err_malloc_nal_buf;
		}
        if(previous_nal_size)
            memcpy(buf, t->p.nal_buffer, previous_nal_size);

        intptr_t delta = buf - t->p.nal_buffer;
        for(i = 0; i < start; i++)
            t->p.out.nal[i].p_payload += delta;

        free(t->p.nal_buffer);
        t->p.nal_buffer = buf;
        t->p.nal_buffer_size = necessary_size;
    }

    return 0;

err_malloc_nal_buf:
	return -1;
}

static int i264e_encoder_encapsulate_nals(i264e_thread_t *t, int start)
{
	int i = 0;
    int nal_size = 0, previous_nal_size = 0, nal_buffer_size = 0;

    for(i = 0; i < start; i++)
        previous_nal_size += t->p.out.nal[i].i_payload;

    for(i = start; i < t->p.out.i_nal; i++)
        nal_size += t->p.out.nal[i].i_payload;

    /* Worst-case NAL unit escaping: reallocate the buffer if it's too small. */
    int necessary_size = previous_nal_size + nal_size * 3/2 + t->p.out.i_nal * 4 + 4 + 64;
    for(i = start; i < t->p.out.i_nal; i++)
        necessary_size += t->p.out.nal[i].i_padding;
    if(i264e_check_encapsulated_buffer(t, start, previous_nal_size, necessary_size)) {
		i264e_log(t->h, C_LOG_ERROR, "check_encapsulated_buffer failed\n");
		goto err_check_encapsulated_buffer;
	}
	uint8_t *nal_buffer = NULL;
	if (0 == t->h->common.param.b_user_nalbuffer) {
		nal_buffer = t->p.nal_buffer + previous_nal_size;
		nal_buffer_size = t->p.nal_buffer_size - previous_nal_size;
	} else {
		nal_buffer = *(t->p.pic_out.nals_buffer) + previous_nal_size;
		nal_buffer_size = t->p.pic_out.nals_buffer_size - previous_nal_size;
	}

	for(i = start; i < t->p.out.i_nal; i++)
	{
		i264e_nal_encode(t->h, nal_buffer, &t->p.out.nal[i]);
		nal_buffer += t->p.out.nal[i].i_payload;
		if (t->p.icodec_io.bslen == 0) {
			t->p.out.nal[i].i_payload = 0;
		}
	}

	if (0 == t->h->common.param.b_user_nalbuffer)
		nal_size = nal_buffer - (t->p.nal_buffer + previous_nal_size);
	else
		nal_size = nal_buffer - (*(t->p.pic_out.nals_buffer) + previous_nal_size);

	if ((nal_size + previous_nal_size) > nal_buffer_size) {
		i264e_log(t->h, C_LOG_ERROR, "bslen=%d, nal_size=%d > nal_buffer_size=%d\n", t->p.icodec_io.bslen, nal_size + previous_nal_size, nal_buffer_size);
		assert(nal_size + previous_nal_size <= nal_buffer_size);
	}

	return nal_size;

err_check_encapsulated_buffer:
	return -1;
}

/****************************************************************************
 * i264e_encoder_headers:
 ****************************************************************************/
int i264e_encoder_headers(i264e_t *h, i264e_nal_t **pp_nal, int *pi_nal)
{
    int frame_size = 0;
    /* init bitstream context */
    h->thread.p.out.i_nal = 0;
    bs_init(&h->thread.p.out.bs, h->thread.p.out.p_bitstream, h->thread.p.out.i_bitstream);

    /* Write SEI, SPS and PPS. */

    /* generate sequence parameters */
    i264e_nal_start(&h->thread, I264E_NAL_SPS, I264E_NAL_PRIORITY_HIGHEST );
    i264e_sps_write(&h->thread.p.out.bs, h->common.sps );
    if(i264e_nal_end(&h->thread)) {
		i264e_log(h, C_LOG_ERROR, "nal sps end failed\n");
		goto err_nal_sps_end;
	}

    /* generate picture parameters */
    i264e_nal_start(&h->thread, I264E_NAL_PPS, I264E_NAL_PRIORITY_HIGHEST );
    i264e_pps_write(&h->thread.p.out.bs, h->common.sps, h->common.pps );
    if(i264e_nal_end(&h->thread)) {
		i264e_log(h, C_LOG_ERROR, "nal pps end failed\n");
		goto err_nal_pps_end;
	}

#ifdef SIGN_I264E_VERSION
    /* identify ourselves */
    i264e_nal_start(&h->thread, I264E_NAL_SEI, I264E_NAL_PRIORITY_DISPOSABLE);
    if(i264e_sei_version_write(&h->thread, &h->thread.p.out.bs)) {
		i264e_log(h, C_LOG_ERROR, "sei_version_write failed\n");
		goto err_sei_version_write;
	}
    if(i264e_nal_end(&h->thread)) {
		i264e_log(h, C_LOG_ERROR, "sei_version_write end failed\n");
		goto err_sei_version_write_end;
	}
#endif

    frame_size = i264e_encoder_encapsulate_nals(&h->thread, 0);
    if(frame_size < 0) {
		i264e_log(h, C_LOG_ERROR, "encoder_encapsulate_nals failed\n");
		goto err_encoder_encapsulate_nals;
	}

    /* now set output*/
    *pi_nal = h->thread.p.out.i_nal;
    *pp_nal = &h->thread.p.out.nal[0];
    h->thread.p.out.i_nal = 0;

    return frame_size;

err_encoder_encapsulate_nals:
#ifdef SIGN_I264E_VERSION
err_sei_version_write_end:
err_sei_version_write:
#endif
err_nal_pps_end:
err_nal_sps_end:
	return -1;
}

static int i264e_slice_write_end(i264e_thread_t *t)
{
	int i = 0;

	if((t->h->common.param.i_threads > 1) && (t->p.b_oldest_thread_used)) {
		i264e_thread_notify_wait(t->p.threads_info.oldest, &t->p.b_oldest_thread_used);
	}

	if((t->h->common.param.i_threads > 1) && (t->p.b_thread_used)) {
		memcpy(&t->op, &t->p.threads_info.oldest->op, sizeof(i264e_thread_output_t));
	}

    t->p.pic_out.nals_buffer = t->p.pic_in.nals_buffer;
    if ((t->p.frame_size = i264e_encoder_encapsulate_nals(t, 0)) < 0) {
		i264e_log(t->h, C_LOG_ERROR, "encoder_encapsulate_nals failed\n");
		goto err_encoder_encapsulate_nals;
	}
    /* Set output picture properties */
    t->p.pic_out.i_qpplus1 = t->p.i_global_qp;
    t->p.pic_out.b_force_idr = t->p.i_type == I264E_TYPE_IDR;
    t->p.pic_out.i_pic_struct = t->p.fenc->i_pic_struct;

    t->p.pic_out.i_pts = t->p.fdec->i_pts;
    t->p.pic_out.i_dts = t->p.fdec->i_dts;
    t->p.pic_out.timestamp = t->p.fdec->timestamp;

    if (t->p.pic_out.i_pts < t->p.pic_out.i_dts) {
        i264e_log(t->h, C_LOG_WARNING, "invalid DTS: PTS is less than DTS\n");
	}

    t->p.pic_out.opaque = t->p.fenc->opaque;
    t->p.pic_out.img.i_csp = t->p.fdec->i_csp;
    t->p.pic_out.img.i_plane = 2;
    for (i = 0; i < t->p.pic_out.img.i_plane; i++) {
        t->p.pic_out.img.i_stride[i] = t->p.fdec->i_stride[i] * sizeof(pixel);
        t->p.pic_out.img.plane[i] = (uint8_t *)t->p.fdec->plane[i];
    }

	if ((t->h->common.param.rc.i_rc_method != I264E_RC_CQP) && (i264e_ratecontrol_end(t) < 0)) {
		i264e_log(t->h, C_LOG_ERROR, "ratecontrol_end failed\n");
		goto err_ratecontrol_end;
	}
    t->p.pic_out.hrd_timing = t->p.fenc->hrd_timing;

	/* Slice stat */
	double dur = (double)1/((double)t->h->common.param.i_fps_num/t->h->common.param.i_fps_den);
	int type = i264e_ftype_to_stype(t->p.i_type);
	t->op.stat.i_frame_count[type]++;
	t->op.stat.i_frame_size[type] += t->p.frame_size;

	t->op.stat.f_frame_duration[type] += dur;
    if (t->h->common.param.analyse.b_psnr) {
	    int64_t ssd[3] = {
			t->p.stat.frame.i_ssd[0],
			t->p.stat.frame.i_ssd[1],
			t->p.stat.frame.i_ssd[2],
	    };
	    int luma_size = t->h->common.param.i_width * t->h->common.param.i_height;
	    int chroma_size = CHROMA_SIZE(luma_size);
	    t->p.pic_out.prop.f_psnr[0] = i264e_psnr(ssd[0], luma_size);
	    t->p.pic_out.prop.f_psnr[1] = i264e_psnr(ssd[1], chroma_size);
	    t->p.pic_out.prop.f_psnr[2] = i264e_psnr(ssd[2], chroma_size);
	    t->p.pic_out.prop.f_psnr_avg = i264e_psnr(ssd[0] + ssd[1] + ssd[2],
				luma_size + chroma_size*2);


	    t->op.stat.f_ssd_global[type]   += dur * (ssd[0] + ssd[1] + ssd[2]);
	    t->op.stat.f_psnr_average[type] += dur * t->p.pic_out.prop.f_psnr_avg;
	    t->op.stat.f_psnr_mean_y[type]  += dur * t->p.pic_out.prop.f_psnr[0];
	    t->op.stat.f_psnr_mean_u[type]  += dur * t->p.pic_out.prop.f_psnr[1];
	    t->op.stat.f_psnr_mean_v[type]  += dur * t->p.pic_out.prop.f_psnr[2];
	}

    if(t->h->common.param.analyse.b_ssim)
    {
		/* TODO */
	}
	/* recover c2g buckup */
	if (t->p.fenc->plane[1] == t->h->common.reconfig.empty)
		t->p.fenc->plane[1] = t->p.uvplane_bak;
	if (t->h->common.param.i_threads == 1) {
		i264e_thread_slice_notify_send(t);
	}

	return t->p.frame_size;

err_ratecontrol_end:
err_encoder_encapsulate_nals:
	return -1;
}

static void *i264e_slice_write(void *arg)
{
	i264e_thread_t *t = arg;
	uint8_t *fbs = NULL;
	uint32_t bslen = 0;
	int b_reset_to_idr = 0;
	int ret_io = 0;
	unsigned int ivdc_overflow;

	i264e_pthread_mutex_lock(&t->p.mutex);
	t->p.b_thread_active = 1;
	t->p.b_thread_used = 1;
	i264e_pthread_mutex_unlock(&t->p.mutex);

	if (t->h->common.param.hskip.b_black_enhance && (t->p.pic_out.fsktype == C_FS_ENHANCE)) {
		if (i264e_icodec_black(t) < 0) {
			i264e_log(t->h, C_LOG_ERROR, "i264e_icodec_black failed\n");
			goto err_i264e_icodec_black;
		} else {
			goto hwicodec_finish_point;
		}
	}

	if (t->h->common.param.denoise.i_dn_type == I264E_DN_NONE) {
		t->p.b_thread_dnstage = DENOISE_STAGE_NONE;
	} else {
		t->p.b_thread_dnstage = DENOISE_STAGE_ENC_EXT;
	}

	if ((t->p.b_thread_dnstage == DENOISE_STAGE_ENC_EXT) && (0 == t->h->common.param.b_buf_share)) {
		int org_global_qp = t->p.i_global_qp;
		int org_sh_type = t->p.sh.i_type;
		if (t->h->common.param.denoise.i_dn_type == I264E_DN_II) {
			t->p.i_global_qp = t->h->common.param.denoise.i_itype_qp;
			t->p.sh.i_type = I264E_SLICE_TYPE_I;
		} else if (t->h->common.param.denoise.i_dn_type == I264E_DN_IP) {
			if (org_sh_type == I264E_SLICE_TYPE_I) {
				t->p.i_global_qp = t->h->common.param.denoise.i_itype_qp;
			} else {
				t->p.i_global_qp = t->h->common.param.denoise.i_ptype_qp;
			}
		}
		if (t->h->common.param.b_cabac) {
			i264e_cabac_context_init(t, &t->p.cabac, t->p.sh.i_type, c_clip3(t->p.i_global_qp, 0, 51 ), t->p.sh.i_cabac_init_idc);
		}
		if (i264e_icodec_enc(t) < 0) {
			i264e_log(t->h, C_LOG_ERROR, "icodec_enc dn failed\n");
			goto err_icodec_enc_dn;
		}
#ifdef CONFIG_SINGLE_BS
		/*由于ivdc VPU0_done_fifo寄存器每编译一帧缓存一次，最多缓存8次,读取后fifo才能减1*/
		/*又因为helix有降噪和重编机制，会导致半直通下VPU多次编译，fifo缓存多次，此处读取寄存器是为了清空fifo*/
		ivdc_regrw(0x1320007c, &ivdc_overflow, 0);
        hwicodec_bsbuf_unlock();
#endif
		t->p.i_global_qp = org_global_qp;
		t->p.sh.i_type = org_sh_type;
		t->p.b_thread_dnstage = DENOISE_STAGE_EXT_ENC;
	}
	if (t->h->common.param.b_cabac) {
		i264e_cabac_context_init(t, &t->p.cabac, t->p.sh.i_type, c_clip3(t->p.i_global_qp, 0, 51 ), t->p.sh.i_cabac_init_idc);
	}

	ret_io = i264e_icodec_enc(t);
	if (ret_io < 0) {
		i264e_log(t->h, C_LOG_ERROR, "icodec_enc failed\n");
		goto err_icodec_enc;
	}

	b_reset_to_idr = i264e_decide_reset_slice_to_idr(t->h, t);
	if (b_reset_to_idr < 0) {
		i264e_log(t->h, C_LOG_ERROR, "i264e_decide_reset_slice_to_idr failed\n");
		goto err_decide_reset_slice_to_idr;
	}

	if (ret_io == 0x2) {
		if (t->h->cur->ip.i_frame != 0) {
			t->h->cur->ip.i_frame -= 1;
		}
	}

	while ((b_reset_to_idr || (i264e_ratecontrol_is_reenc(t) > 0)) && (0 == t->h->common.param.b_buf_share)) {
        if (!b_reset_to_idr && t->p.b_change_ref) {
            memset(&t->p.reenc_tmp_info, 0, sizeof(t->p.reenc_tmp_info));
            t->p.reenc_tmp_info.old_fref = t->p.fref;
            t->p.reenc_tmp_info.cur_fref = i264e_get_prev_fram_dec(t->h, t);
            if (t->p.reenc_tmp_info.cur_fref != NULL) {
                if (t->p.tmp_bs_size < t->p.icodec_io.bslen * 2) {
                    if (t->p.tmp_bs_space) {
                        free(t->p.tmp_bs_space);
                        t->p.tmp_bs_size = 0;
                        t->p.tmp_bs_space = NULL;
                    }
                    t->p.tmp_bs_space = calloc(1, t->p.icodec_io.bslen * 2);
                    if (t->p.tmp_bs_space == NULL) {
                        t->p.b_change_ref = 0;
                        i264e_log(t->h, C_LOG_ERROR, "calloc tmp_bs_space failed\n");
                        goto err_calloc_tmp_bs_space;
                    }
                    t->p.tmp_bs_size = t->p.icodec_io.bslen * 2;
                }
                memcpy(t->p.tmp_bs_space, t->p.fbs, t->p.icodec_io.bslen);
                t->p.reenc_tmp_info.old_bs = t->p.tmp_bs_space;
                t->p.reenc_tmp_info.i_old_bslen = t->p.icodec_io.bslen;
                t->p.fref = t->p.reenc_tmp_info.cur_fref;
            } else {
                t->p.b_change_ref = 0;
            }
        }
#ifdef CONFIG_SINGLE_BS
	hwicodec_bsbuf_unlock();
#endif
		t->p.sh.i_qp = SPEC_QP(t->p.i_global_qp);
		t->p.sh.i_qp_delta = t->p.sh.i_qp - t->h->common.pps->i_pic_init_qp;
		int deblock_thresh = t->p.sh.i_qp + 2 * C_MIN(t->h->common.param.i_deblocking_filter_alphac0, t->h->common.param.i_deblocking_filter_beta);
		/* If effective qp <= 15, deblocking would have no effect anyway */
		if (t->h->common.param.b_deblocking_filter && (15 < deblock_thresh))
			t->p.sh.i_disable_deblocking_filter_idc = 0;
		else
			t->p.sh.i_disable_deblocking_filter_idc = 1;
		if (t->h->common.param.b_cabac) {
			i264e_cabac_context_init(t, &t->p.cabac, t->p.sh.i_type, c_clip3(t->p.i_global_qp, 0, 51 ), t->p.sh.i_cabac_init_idc);
		}

#ifdef CONFIG_SINGLE_BS
		/*由于ivdc VPU0_done_fifo寄存器每编译一帧缓存一次，最多缓存8次,读取后fifo才能减1*/
		/*又因为helix有降噪和重编机制，会导致半直通下VPU多次编译，fifo缓存多次，此处读取寄存器是为了清空fifo*/
		ivdc_regrw(0x1320007c, &ivdc_overflow, 0);
#endif
		if (i264e_icodec_enc(t) < 0) {
			i264e_log(t->h, C_LOG_ERROR, "repeate icodec_enc failed\n");
			goto err_icodec_enc_repeate;
		}

        if (!b_reset_to_idr && t->p.b_change_ref) {
            t->p.reenc_tmp_info.cur_bs = t->p.fbs;
            t->p.reenc_tmp_info.i_cur_bslen = t->p.icodec_io.bslen;
            i264e_ratecontrol_is_reenc_end(t);      //used to judge whether to use reencode bs
            if (t->p.b_change_ref) {
                i264e_dec_ref_pic_remark(t->h, t);
            } else {
                t->p.fref = t->p.reenc_tmp_info.old_fref;
                memcpy(t->p.fbs, t->p.reenc_tmp_info.old_bs, t->p.reenc_tmp_info.i_old_bslen);
                t->p.icodec_io.bslen = t->p.reenc_tmp_info.i_old_bslen;
            }
            t->p.b_change_ref = 0;
            memset(&t->p.reenc_tmp_info, 0, sizeof(t->p.reenc_tmp_info));
        }

		b_reset_to_idr = 0;
	}
	if (t->h->common.param.analyse.b_psnr) {
		i264e_param_t *pa = &(t->h->common.param);
		t->p.stat.frame.i_ssd[0] =
		t->p.stat.frame.i_ssd[1] =
		t->p.stat.frame.i_ssd[2] = 0;

		if(pa->i_csp == C_CSP_T420)
		{
			int i1;
			for( i1=0; i1<3; i1++ )
				t->p.stat.frame.i_ssd[i1] += i264e_pixel_ssd_wxh_tile((uint8_t *)t->p.fdec->plane[!!i1], (uint8_t *)t->p.fenc->plane[!!i1], t->h->common.sps->i_mb_width, t->h->common.sps->i_mb_height, i1 == 0 ? 'y' : (i1 == 1 ? 'u' : 'v'), 1);
		}
		else if(pa->i_csp == C_CSP_NV21)
		{
			int i1;
			for( i1=0; i1<3; i1++ )
				t->p.stat.frame.i_ssd[i1] += i264e_pixel_ssd_wxh_nv12((uint8_t *)t->p.fdec->plane[!!i1], (uint8_t *)t->p.fenc->plane[!!i1], t->h->common.sps->i_mb_width, t->h->common.sps->i_mb_height, i1 == 0 ? 'y' : (i1 == 1 ? 'v' : 'u'), 1);
		}
		else if(pa->i_csp == C_CSP_NV12)
		{
			int i1;
			for( i1=0; i1<3; i1++ ) {
				t->p.stat.frame.i_ssd[i1] += i264e_pixel_ssd_wxh_nv12((uint8_t *)t->p.fdec->plane[!!i1], (uint8_t *)t->p.fenc->plane[!!i1], t->h->common.sps->i_mb_width, t->h->common.sps->i_mb_height, i1 == 0 ? 'y' : (i1 == 1 ? 'u' : 'v'), 1);
			}
		}
	}

	if ((t->h->common.param.b_enc_save > 0) && (t->p.b_thread_dnstage == DENOISE_STAGE_NONE)) {
		//i264e_log(t->h, C_LOG_DEBUG, "save enc none start\n");
		c_save_to_file(t->h->common.param.b_enc_save, t->p.fenc->plane[0], t->h->common.sps->i_mb_width * 16 *
				t->h->common.sps->i_mb_height * 16);
		c_save_to_file(t->h->common.param.b_enc_save, t->p.fenc->plane[1], t->h->common.sps->i_mb_width * 16 *
				t->h->common.sps->i_mb_height * 8);
		//i264e_log(t->h, C_LOG_DEBUG, "save enc none end\n");
	} else if ((t->h->common.param.b_enc_save > 0) && (t->p.b_thread_dnstage == DENOISE_STAGE_EXT_ENC)) {
		//i264e_log(t->h, C_LOG_DEBUG, "save enc ext_enc start\n");
		c_save_to_file(t->h->common.param.b_enc_save, t->h->common.extframe[t->ip.i_thread_phase].plane[0],
				t->h->common.sps->i_mb_width * 16 * t->h->common.sps->i_mb_height * 16);
		c_save_to_file(t->h->common.param.b_enc_save, t->h->common.extframe[t->ip.i_thread_phase].plane[1],
				t->h->common.sps->i_mb_width * 16 * t->h->common.sps->i_mb_height * 8);
		//i264e_log(t->h, C_LOG_DEBUG, "save enc ext_enc end\n");
	}

	if ((t->h->common.param.b_ref_save > 0) && (t->p.sh.i_type != I264E_SLICE_TYPE_I)) {
		//i264e_log(t->h, C_LOG_DEBUG, "save ref start\n");
		c_save_to_file(t->h->common.param.b_ref_save, t->p.fref->plane[0], (t->h->common.sps->i_mb_width + 2) * 16 *
				t->h->common.sps->i_mb_height * 16);
		c_save_to_file(t->h->common.param.b_ref_save, t->p.fref->plane[1], (t->h->common.sps->i_mb_width + 2) * 16 *
				t->h->common.sps->i_mb_height * 8);
		//i264e_log(t->h, C_LOG_DEBUG, "save ref end\n");
	}

	if ((t->h->common.param.b_dec_save > 0)) {
		//i264e_log(t->h, C_LOG_DEBUG, "save dec start\n");
		c_save_to_file(t->h->common.param.b_dec_save, t->p.fdec->plane[0], (t->h->common.sps->i_mb_width + 2) * 16 *
				t->h->common.sps->i_mb_height * 16);
		c_save_to_file(t->h->common.param.b_dec_save, t->p.fdec->plane[1], (t->h->common.sps->i_mb_width + 2) * 16 *
				t->h->common.sps->i_mb_height * 8);
		//i264e_log(t->h, C_LOG_DEBUG, "save dec end\n");
	}

hwicodec_finish_point:
	fbs = t->p.fbs;
	bslen = t->p.icodec_io.bslen;

	if (t->h->common.param.b_bs_save > 0) {
		//i264e_log(t->h, C_LOG_DEBUG, "save bs start\n");
		c_save_to_file(t->h->common.param.b_bs_save, fbs, bslen);
		//i264e_log(t->h, C_LOG_DEBUG, "save bs end\n");
	}


    bs_realign(&t->p.out.bs);
	i264e_nal_start(t, t->p.i_nal_type, t->p.i_nal_ref_idc);

	i264e_slice_header_write(&t->p.out.bs, &t->p.sh, t->p.i_nal_ref_idc);
    if (t->h->common.param.b_cabac) {
        /* alignment needed */
        bs_align_1(&t->p.out.bs);
        i264e_cabac_encode_init(&t->p.cabac, t->p.out.bs.p, t->p.out.bs.p_end);
    }

	int write_bslen = bslen;
	if ((t->p.out.bs.p_end - t->p.out.bs.p - 4) < bslen) {
		write_bslen = t->p.out.bs.p_end - t->p.out.bs.p - 4;
		i264e_log(t->h, C_LOG_WARNING, "frame type=%d, bitsteam buf size %d is too small to real bs %d, force bslen to %d\n", t->p.i_type, t->p.out.i_bitstream, bslen, write_bslen);
	}

	if (t->p.out.bs.i_left == 32) {
		memcpy(t->p.out.bs.p, fbs, write_bslen);
		t->p.out.bs.p += write_bslen;
	} else {
		int length = write_bslen;
		volatile unsigned char *bit_ptr = (volatile unsigned char *)fbs;
		while(length--)
			bs_write(&t->p.out.bs, 8, *bit_ptr++);
		bs_rbsp_trailing(&t->p.out.bs);
	}

#ifdef CONFIG_SINGLE_BS
	hwicodec_bsbuf_unlock();
#endif

	if(i264e_nal_end(t)) {
		i264e_log(t->h, C_LOG_ERROR, "nal_end rbsp failed\n");
		goto err_nal_rbsp_end;
	}

	return (void *)i264e_slice_write_end(t);

err_nal_rbsp_end:
err_icodec_enc_repeate:
err_calloc_tmp_bs_space:
err_decide_reset_slice_to_idr:
err_icodec_enc:
err_icodec_enc_dn:
err_i264e_icodec_black:
	return (void *)-1;
}

int i264e_encode(i264e_t *h, i264e_pic_t *pic_in)
{
	int overhead = NALU_OVERHEAD, i = 0;

	if (h->common.param.i_threads > 1) {
		int i_thread_phase = h->cur->ip.i_thread_phase;
		h->cur = h->threads[(i_thread_phase + 1) % h->common.param.i_threads];
		if (h->cur->p.b_thread_used) {
			i264e_thread_notify_wait(h->cur, &h->threads[i_thread_phase]->p.b_oldest_thread_used);
		}
		h->cur->p.threads_info.prev = h->threads[i_thread_phase];
		h->cur->p.threads_info.cur = h->cur;
		i_thread_phase = (i_thread_phase + 1) % h->common.param.i_threads;
		h->cur->p.threads_info.oldest = h->threads[(i_thread_phase + 1) % h->common.param.i_threads];
		memcpy(&h->cur->ip, &h->cur->p.threads_info.prev->ip, sizeof(i264e_thread_input_t));
		h->cur->ip.i_thread_phase = i_thread_phase;
		i264e_pthread_mutex_lock(&h->cur->p.mutex);
		h->cur->p.b_oldest_thread_used = h->cur->p.threads_info.oldest->p.b_thread_used;
		i264e_pthread_mutex_unlock(&h->cur->p.mutex);
    } else {
         h->cur = &h->thread;
		 if (h->cur->p.b_thread_used) {
			 i264e_thread_notify_wait(h->cur, &h->cur->p.b_oldest_thread_used);
		 }
         h->cur->p.threads_info.cur = h->cur->p.threads_info.oldest = h->cur->p.threads_info.prev = &h->thread;
    }

	if (i264e_update_fenc(h, h->cur->p.fenc, pic_in) < 0) {
		i264e_log(h, C_LOG_ERROR, "update_fenc failed\n");
		goto err_update_fenc;
	}

	if(h->cur->p.fpad) {
		h->cur->p.fpad->plane[0] = pic_in->pad_img.plane[0];
		h->cur->p.fpad->plane[1] = pic_in->pad_img.plane[1];
	}

	h->cur->p.prev_i_frame = h->cur->ip.i_frame;
	h->cur->p.sh.b_ref_pic_list_reordering[0] = 0;
	h->cur->p.sh.b_ref_pic_list_reordering[1] = 0;
	h->cur->p.sh.i_mmco_command_count = 0;
	h->cur->p.pic_in.i_qpplus1 = pic_in->i_qpplus1 = h->cur->p.i_global_qp;

	if (i264e_decide_slice_type_and_rd(h, h->cur) < 0) {
		i264e_log(h, C_LOG_ERROR, "decide_rdindex failed\n");
		goto err_decide_rdindex;
	}

	if (i264e_ratecontrol_start(h->cur) < 0) {
		i264e_log(h, C_LOG_ERROR, "ratecontrol_start failed\n");
		goto err_ratecontrol;
	}
	i264e_decide_slice_qp(h, h->cur);

	if (h->cur->p.i_type == I264E_TYPE_IDR) {
		if (h->common.param.i_threads > 1) {
			i264e_threadpool_wait_all(h);
		}
		if (i264e_idr_reconfig(h) < 0) {
			i264e_log(h, C_LOG_WARNING, "i264e_idr_reconfig failed\n");
		}
	}
	if (i264e_reconfig(h) < 0) {
		i264e_log(h, C_LOG_WARNING, "i264e_reconfig failed\n");
	}

	if (h->common.param.hskip.b_black_enhance && (h->cur->p.pic_out.fsktype == C_FS_ENHANCE)) {
		h->cur->p.i_global_qp = 0;
	}

	h->cur->p.pic_in.i_qpplus1 = pic_in->i_qpplus1 = h->cur->p.i_global_qp;

    h->cur->p.fdec->i_poc =
    h->cur->p.fenc->i_poc = 2 * h->cur->ip.i_frame;

	/* TO SPS, PPS */
	if (h->cur->p.i_type == I264E_TYPE_IDR) {
		h->cur->p.i_nal_type = I264E_NAL_SLICE_IDR;
		h->cur->p.i_nal_ref_idc = I264E_NAL_PRIORITY_HIGHEST;
        h->cur->p.sh.i_type = I264E_SLICE_TYPE_I;
	} else if (h->cur->p.i_type == I264E_TYPE_P) {
		h->cur->p.i_nal_type = I264E_NAL_SLICE;
		h->cur->p.i_nal_ref_idc = I264E_NAL_PRIORITY_HIGH;
        h->cur->p.sh.i_type = I264E_SLICE_TYPE_P;
	}

    h->cur->p.fdec->i_pts = h->cur->p.fenc->i_pts;
    h->cur->p.fdec->i_dts = h->cur->p.fenc->i_pts;
    h->cur->p.fdec->timestamp = h->cur->p.fenc->timestamp;

	bs_init(&h->cur->p.out.bs, h->cur->p.out.p_bitstream, h->cur->p.out.i_bitstream);
	h->cur->p.out.i_nal = 0;

    if(h->cur->p.i_type == I264E_TYPE_IDR) {
        /* Write SPS and PPS */
        if(h->common.param.b_repeat_headers) {
            /* generate sequence parameters */
            i264e_nal_start(h->cur, I264E_NAL_SPS, I264E_NAL_PRIORITY_HIGHEST);
            i264e_sps_write(&h->cur->p.out.bs, h->common.sps);
            if (i264e_nal_end(h->cur)) {
				i264e_log(h, C_LOG_ERROR, "nal sps end failed\n");
				goto err_nal_sps_end;
			}
			overhead += h->cur->p.out.nal[h->cur->p.out.i_nal-1].i_payload
				+ h->cur->p.out.nal[h->cur->p.out.i_nal-1].i_padding
				+ NALU_OVERHEAD;

            /* generate picture parameters */
            i264e_nal_start(h->cur, I264E_NAL_PPS, I264E_NAL_PRIORITY_HIGHEST);
            i264e_pps_write(&h->cur->p.out.bs, h->common.sps, h->common.pps);
            if(i264e_nal_end(h->cur)) {
				i264e_log(h, C_LOG_ERROR, "nal pps end failed\n");
				goto err_nal_pps_end;
			}
			overhead += h->cur->p.out.nal[h->cur->p.out.i_nal-1].i_payload
				+ h->cur->p.out.nal[h->cur->p.out.i_nal-1].i_padding
				+ NALU_OVERHEAD;

#ifdef SIGN_I264E_VERSION
            /* identify ourself */
			/* Avid's decoder strictly wants two SEIs for AVC-Intra so we can't insert the i264e SEI */
            i264e_nal_start(h->cur, I264E_NAL_SEI, I264E_NAL_PRIORITY_DISPOSABLE);
            if( i264e_sei_version_write(h->cur, &h->cur->p.out.bs)) {
				i264e_log(h, C_LOG_ERROR, "sei_version_write failed\n");
				goto err_sei_version_write;
			}
            if(i264e_nal_end(h->cur)) {
				i264e_log(h, C_LOG_ERROR, "sei_version_write end failed\n");
				goto err_sei_version_write_end;
			}
			overhead += h->cur->p.out.nal[h->cur->p.out.i_nal-1].i_payload
				+ SEI_OVERHEAD;
#endif

        }
    }

    /* write extra sei */
    for (i = 0; i < h->cur->p.fenc->extra_sei.num_payloads; i++) {
        i264e_nal_start(h->cur, I264E_NAL_SEI, I264E_NAL_PRIORITY_DISPOSABLE);
        i264e_sei_write(&h->cur->p.out.bs, h->cur->p.fenc->extra_sei.payloads[i].payload, h->cur->p.fenc->extra_sei.payloads[i].payload_size,
                        h->cur->p.fenc->extra_sei.payloads[i].payload_type);
        if (i264e_nal_end(h->cur) < 0) {
			i264e_log(h, C_LOG_ERROR, "sei_nal_end to extra_sei %d failed\n", i);
			goto err_extra_sei_nal_end;
		}
		overhead += h->cur->p.out.nal[h->cur->p.out.i_nal-1].i_payload + SEI_OVERHEAD;
        if (h->cur->p.fenc->extra_sei.sei_free) {
            h->cur->p.fenc->extra_sei.sei_free(h->cur->p.fenc->extra_sei.payloads[i].payload);
            h->cur->p.fenc->extra_sei.payloads[i].payload = NULL;
        }
    }

    if (h->cur->p.fenc->extra_sei.sei_free) {
        h->cur->p.fenc->extra_sei.sei_free(h->cur->p.fenc->extra_sei.payloads);
        h->cur->p.fenc->extra_sei.payloads = NULL;
        h->cur->p.fenc->extra_sei.sei_free = NULL;
    }

    /* generate sei pic timing */
    if(h->common.sps->vui.b_pic_struct_present || h->common.sps->vui.b_nal_hrd_parameters_present) {
        i264e_nal_start(h->cur, I264E_NAL_SEI, I264E_NAL_PRIORITY_DISPOSABLE);
        i264e_sei_pic_timing_write(h->cur, &h->cur->p.out.bs);
        if(i264e_nal_end(h->cur)) {
			i264e_log(h, C_LOG_ERROR, "sei_pic_timing_write end failed\n");
			goto err_sei_pic_timing_write_end;
		}
		overhead += h->cur->p.out.nal[h->cur->p.out.i_nal-1].i_payload
			+ SEI_OVERHEAD;
    }

	i264e_slice_init(h->cur);

	if (h->common.param.i_threads > 1) {
		i264e_threadpool_run(h->common.threadpool, i264e_slice_write, h->cur);
	} else {
		if(((int)i264e_slice_write(h->cur)) < 0) {
			i264e_log(h, C_LOG_ERROR, "[func:%s,line:%d]i264e_slice_write failed\n",
					__func__, __LINE__);
			goto err_i264e_slice_write;
		}
	}
	return 0;

err_i264e_slice_write:
err_sei_pic_timing_write_end:
err_extra_sei_nal_end:
#ifdef SIGN_I264E_VERSION
err_sei_version_write_end:
err_sei_version_write:
#endif
err_nal_pps_end:
err_nal_sps_end:
err_ratecontrol:
err_decide_rdindex:
err_update_fenc:
	return -1;
}

void i264e_get_bitstream_cleanup_route(void *arg)
{
    i264e_t *h = (i264e_t *)arg;

    if (h->common.param.i_threads > 1) {
        i264e_pthread_mutex_unlock(&h->common.threadpool->done.mutex);
    } else {
        i264e_pthread_mutex_unlock(&h->cur->p.mutex);
    }
}

int i264e_get_bitstream(i264e_t *h, i264e_nal_t **pp_nal, int *pi_nal, i264e_pic_t **pic_in,
		i264e_pic_t **pic_out, void **bshandler, void **thandler)
{
	i264e_thread_t *t = NULL;
	if (h->common.param.i_threads > 1) {
		t = i264e_threadpool_get_done(h->common.threadpool, bshandler);
	} else {
		t = h->cur;
		i264e_thread_slice_notify_wait(t);
	}

    /* End bitstream, set output  */
    *pi_nal = t->p.out.i_nal;
    *pp_nal = t->p.out.nal;
	*pic_in = &t->p.pic_in;
	*pic_out = &t->p.pic_out;
	*thandler = t;

	return t->p.frame_size;
}

int i264e_release_bitstream(i264e_t *h, void *bshandler, void *thandler)
{
	int ret = 0;
	if (h->common.param.i_threads > 1) {
		ret = i264e_threadpool_release_done(h->common.threadpool, bshandler);
	}
	i264e_thread_notify_send(thandler);

	return ret;
}

int i264e_delay_num(i264e_t *h)
{
    int delayed_frames = 0;
	int i = 0;

    if(h->common.param.i_threads > 1) {
        for(i = 0; i < h->common.param.i_threads; i++ ) {
            delayed_frames += h->threads[i]->p.b_thread_active;
		}
    }
	return delayed_frames;
}

int i264e_get_param(i264e_t *h, int param_id, void *param)
{
	int rv = 0;
	switch(param_id) {
		case I264E_RCFG_C2G_ID:
			rv = i264e_reconfig_color2gray_get(h, param);
			break;
		case I264E_RCFG_CUT_ID:
			rv = i264e_reconfig_cut_get(h, param);
			break;
		case I264E_RCFG_ROI_ID:
			rv = i264e_reconfig_roi_get(h, param);
			break;
		case I264E_RCFG_RC_ID:
			rv = i264e_reconfig_rc_get(h, param);
			break;
		case I264E_RCFG_RC_TRIG_ID:
			rv = i264e_reconfig_rc_trig_get(h, param);
			break;
		case I264E_RCFG_FPS_ID:
			rv = i264e_reconfig_fps_get(h, param);
			break;
		case I264E_RCFG_ENIDR_ID:
			rv = i264e_reconfig_enidr_get(h, param);
			break;
		case I264E_RCFG_GOP_ID:
			rv = i264e_reconfig_gop_get(h, param);
			break;
		case I264E_RCFG_DN_ID:
			rv = i264e_reconfig_dn_get(h, param);
			break;
		case I264E_RCFG_HSKIP_ID:
			rv = i264e_reconfig_hskip_get(h, param);
			break;
        case I264E_RCFG_MBRC_ID:
			rv = i264e_mb_rc_get(h, param);
			break;
        case I264E_RCFG_CHGREF_ID:
			rv = i264e_change_ref_get(h, param);
			break;
        case I264E_RCFG_SUPER_ID:
			rv = i264e_super_rc_get(h, param);
			break;
        case I264E_RCFG_TRANS_ID:
			rv = i264e_reconfig_trans_get(h, param);
			break;
        case I264E_RCFG_QPGMODE_ID:
			rv = i264e_reconfig_qpgmode_get(h, param);
			break;
		case I264E_RCFG_RCFILE_ID:
			rv = i264e_reconfig_rcfile_get(h, param);
			break;
		default:
			i264e_log(h, C_LOG_ERROR, "error param id\n");
			rv = -1;
	}
	return rv;
}

int i264e_set_param(i264e_t *h, int param_id, const void *param)
{
	int rv = 0;
	switch(param_id) {
		case I264E_RCFG_C2G_ID:
			rv = i264e_reconfig_color2gray_set(h, param);
			break;
		case I264E_RCFG_CUT_ID:
			rv = i264e_reconfig_cut_set(h, param);
			break;
		case I264E_RCFG_ROI_ID:
			rv = i264e_reconfig_roi_set(h, param);
			break;
		case I264E_RCFG_RC_ID:
			rv = i264e_reconfig_rc_set(h, param);
			break;
		case I264E_RCFG_RC_TRIG_ID:
			rv = i264e_reconfig_rc_trig_set(h, param);
			break;
		case I264E_RCFG_FPS_ID:
			rv = i264e_reconfig_fps_set(h, param);
			break;
		case I264E_RCFG_ENIDR_ID:
			rv = i264e_reconfig_enidr_set(h, param);
			break;
		case I264E_RCFG_GOP_ID:
			rv = i264e_reconfig_gop_set(h, param);
			break;
		case I264E_RCFG_DN_ID:
			rv = i264e_reconfig_dn_set(h, param);
			break;
		case I264E_RCFG_HSKIP_ID:
			rv = i264e_reconfig_hskip_set(h, param);
			break;
		case I264E_RCFG_BENHANCE_ID:
			rv = i264e_reconfig_black_enhance_set(h, param);
			break;
        case I264E_RCFG_MBRC_ID:
			rv = i264e_mb_rc_set(h, param);
			break;
        case I264E_RCFG_CHGREF_ID:
			rv = i264e_change_ref_set(h, param);
			break;
        case I264E_RCFG_SUPER_ID:
			rv = i264e_super_rc_set(h, param);
			break;
        case I264E_RCFG_TRANS_ID:
			rv = i264e_reconfig_trans_set(h, param);
			break;
        case I264E_RCFG_QPGMODE_ID:
			rv = i264e_reconfig_qpgmode_set(h, param);
			break;
		case I264E_RCFG_RCFILE_ID:
			rv = i264e_reconfig_rcfile_set(h,param);
			break;
		default:
			i264e_log(h, C_LOG_ERROR, "error param id\n");
			rv = -1;
	}
	return rv;
}
